#include "bianma.hpp"
#include <iostream>

/// @brief 
/// @param port 
/// @param baud 
bianma::bianma(std::string port, int baud)
     : Serial(port, baud)
{
    open();
    for(int i=0;i<4;i++)
    {
      send_count[i]=0;  rec_count[i]=0;
      current_angle[i]=0;set_angle[i]=0;
    }
    runThreadFunc();
    startTimer(5);
}
void bianma::Set_reset_zero(void)
{
    Reset_flag=1;
}
void bianma::Clr_reset_zero(void)
{
    Reset_flag=0;
}
void bianma::recvMsg(std::vector<u_int8_t> &msg)
{
    // std::stringstream ss;
    // for(auto i: msg)
    // {
    //     ss << toHex(i) << " ";
    // }
    //  std::cout << "recv: " << ss.str() << std::endl;
    // write(msg);
    int value1,value2;
  
    if(rec_ok(msg)==0)
    {
        int id=0,i,flag,index;
        flag=0;
        id=msg[1]*256+msg[2];
        int16_t ag;
        id-=0x100;
        //display_rec_hex(msg);
        //std::cout << "recv:ss " << id<<std::endl;
        for(i=0;i<4;i++)
        {
            if(id==bianma_id[i]){flag=1;index=i;break; }
        }
        if(flag==1)
        {
             ag=(msg[3]<<8)+msg[4];
             if(ag>=16384)ag=ag-32768;  
             if((ag==32768)|(ag==-32768))ag=0;
             current_angle[index]=ag;
        }
        //
    }

}
int bianma::rec_ok(std::vector<u_int8_t> &msg)
{
    int flag=0;
    u_int8_t ch=0;
    if(msg[0]!=0xaa)return 1;
    if(msg.size()!=12)return 2;
    for(int i=0;i<11;i++)
        ch+=msg[i];
    if(ch==msg[11])    
        return flag;
      else return 3;  
}
void bianma::startTimer(int ms_s)
{
     interval = std::chrono::milliseconds(ms_s);   
     if (!running) {
            running = true;
            auto thread = std::thread([this]() {
                while (running) {
                //    callback(); // 执行回调
                   // if(sendbuf_vect.size()!=0)
                   // {
                        if(circle_count==4)circle_count=0;
                        sendbuf_vect.clear();
                        set_send_v(Reset_flag,circle_count);
                        write(sendbuf_vect);
                        send_count[circle_count]++;
                        // if(Reset_flag==1)  std::cout << "send clr"<< std::endl;
                        //     else std::cout << "send const"<< std::endl;
                        circle_count++;
                  //  }
                    std::this_thread::sleep_for(interval);
                }
            });
            thread.detach(); // 让定时器在后台运行，防止主线程阻塞
        }
}
void bianma::set_send_v(int n,int id)
{
    u_char ch;
    switch(n)
    {
        case 0:  //读编码器角度
        sendbuf_vect.push_back(0xaa);
        ch=bianma_id[id]>>8;
        sendbuf_vect.push_back(ch);
        ch=bianma_id[id]&0xff;
        sendbuf_vect.push_back(ch);
        sendbuf_vect.push_back(0x01);
        sendbuf_vect.push_back(0x00);
        sendbuf_vect.push_back(0x00);
        sendbuf_vect.push_back(0x00);
        sendbuf_vect.push_back(0x00);
        sendbuf_vect.push_back(0x00);
        sendbuf_vect.push_back(0x00);
        sendbuf_vect.push_back(0x00);
        ch=0x0bd+(id&0xff);
        sendbuf_vect.push_back(ch);
        break; 
        //编码器置零
        case 1:
        sendbuf_vect.push_back(0xaa);
        ch=bianma_id[id]>>8;
        sendbuf_vect.push_back(ch);
        ch=bianma_id[id]&0xff;
        sendbuf_vect.push_back(ch);
        sendbuf_vect.push_back(0x02);
        sendbuf_vect.push_back(0x00);
        sendbuf_vect.push_back(0x00);
        sendbuf_vect.push_back(0x00);
        sendbuf_vect.push_back(0x00);
        sendbuf_vect.push_back(0x00);
        sendbuf_vect.push_back(0x00);
        sendbuf_vect.push_back(0x00);
        ch= 0x0be + (id&0xff);
        sendbuf_vect.push_back(ch);
        break;
    }
}

